# coding=utf-8
import argparse
import math
import time
from naoqi import ALProxy

IP = "192.168.1.5"
PORT = 9559


motionProxy = ALProxy("ALMotion", IP, PORT)
postureProxy = ALProxy("ALRobotPosture", IP, PORT)

# motionProxy.wakeUp()

motionProxy.setStiffnesses("Body", 1.0)

time.sleep(1.0)

postureProxy.goToPosture("StandInit", 0.5)

# motionProxy.moveInit()

maxstepx = 0.03
maxstepy = 0.14
maxsteptheta = 0.3
maxstepfrequency = 0.6
stepheight = 0.02
torsowx = 0.0
torsowy = 0.0
moveConfig = [["MaxStepX", maxstepx],
              ["MaxStepY", maxstepy],
              ["MaxStepTheta", maxsteptheta],
              ["MaxStepFrequency", maxstepfrequency],
              ["StepHeight", stepheight],
              ["TorsoWx", torsowx],
              ["TorsoWy", torsowy]]

#motionProxy.moveTo(0, 0,-0.65, moveConfig)
#time.sleep(0)
motionProxy.moveTo(0.8,0,0 ,moveConfig)
time.sleep(0)
motionProxy.moveTo(0, 0,1.2, moveConfig)
time.sleep(0)
#motionProxy.move(0.5, 0.5, 0, moveConfig)
motionProxy.moveTo(1.8, 0,0, moveConfig)
time.sleep(0)
motionProxy.moveTo(0, 0,1.5, moveConfig)
time.sleep(0)
motionProxy.moveTo(1.4, 0,0, moveConfig)
time.sleep(0)
motionProxy.moveTo(0, 0,-1.5, moveConfig)
time.sleep(0)
motionProxy.moveTo(0.75, 0,0, moveConfig)
time.sleep(0)
motionProxy.stopMove()

postureProxy.goToPosture("Crouch", 0.5)